/********************************************************************
                           3D-Soft-Engine

  File : unit_tests.cpp
  Description : Unit tests on 3D Maths
  License : GNU General Public License 3 or +
  Author : Guillaume Delhumeau <guillaume.delhumeau@gmail.com>


*********************************************************************/


#include "unit_tests.h"
#include <iostream>

#include "Matrix.h"
#include "Vector.h"

using namespace std;

void test_matrix()
{
	cout << "---- Test Matrix ----" << endl;
	
	Matrix4f matA;
	matA.Set(0,0,56);
	matA.Set(1,0,1);
	matA.Set(2,0,2);
	matA.Set(3,0,7);
	
	matA.Set(0,1,36);
	matA.Set(1,1,12);
	matA.Set(2,1,0);
	matA.Set(3,1,4);
	
	matA.Set(0,2,36);
	matA.Set(1,2,2);
	matA.Set(2,2,74);
	matA.Set(3,2,3);
	
	matA.Set(0,3,10);
	matA.Set(1,3,45);
	matA.Set(2,3,6);
	matA.Set(3,3,9);
	
	Matrix4f matB;
	matB.Set(0,0,1);
	matB.Set(1,0,9);
	matB.Set(2,0,3);
	matB.Set(3,0,55);
	
	matB.Set(0,1,12);
	matB.Set(1,1,6);
	matB.Set(2,1,32);
	matB.Set(3,1,6);
	
	matB.Set(0,2,77);
	matB.Set(1,2,8);
	matB.Set(2,2,0);
	matB.Set(3,2,2);
	
	matB.Set(0,3,4);
	matB.Set(1,3,88);
	matB.Set(2,3,63);
	matB.Set(3,3,76);
	
	Matrix4f matC = matA * matB;
	if(	matC.Get(0,0) != 250 ||
			matC.Get(1,0) != 1142 ||
			matC.Get(2,0) != 641 ||
			matC.Get(3,0) != 3622 ||
			matC.Get(0,1) != 196 ||
			matC.Get(1,1) != 748 ||
			matC.Get(2,1) != 744 ||
			matC.Get(3,1) != 2356 ||
			matC.Get(0,2) != 5770 ||
			matC.Get(1,2) != 1192 ||
			matC.Get(2,2) != 361 ||
			matC.Get(3,2) != 2368 ||
			matC.Get(0,3) != 1048 ||
			matC.Get(1,3) != 1200 ||
			matC.Get(2,3) != 2037 ||
			matC.Get(3,3) != 1516	)
	{
		cout << "Error matrice multiplication !" << endl;
		cout << matC.ToString() << endl;
	}

	matC = matB * matA;
	if(	matC.Get(0,0) != 1038 ||
			matC.Get(1,0) != 2590 ||
			matC.Get(2,0) != 554 ||
			matC.Get(3,0) != 547 ||
			matC.Get(0,1) != 2100 ||
			matC.Get(1,1) != 418 ||
			matC.Get(2,1) != 2428 ||
			matC.Get(3,1) != 258 ||
			matC.Get(0,2) != 4620 ||
			matC.Get(1,2) != 263 ||
			matC.Get(2,2) != 166 ||
			matC.Get(3,2) != 589 ||
			matC.Get(0,3) != 6420 ||
			matC.Get(1,3) != 4606 ||
			matC.Get(2,3) != 5126 ||
			matC.Get(3,3) != 1253	)
	{
		cout << "Error matrice multiplication !" << endl;
		cout << matC.ToString() << endl;
	}
	
	Vector4f vec;
	vec.Set(23, 5, 9, 7);
	
	Vector4f vec_r = matA * vec;
	if(	vec_r.GetX() != 1360 ||
			vec_r.GetY() != 916 ||
			vec_r.GetZ() != 1525 ||
			vec_r.GetW() != 572 )
	{
		cout << "Error matrice * vector !" << endl;
		cout << vec_r.ToString() << endl;
	}
	
	vec_r = vec * matA;
	if(	vec_r.GetX() != 1360 ||
			vec_r.GetY() != 916 ||
			vec_r.GetZ() != 1525 ||
			vec_r.GetW() != 572 )
	{
		cout << "Error matrice * vector !" << endl;
		cout << vec_r.ToString() << endl;
	}
	
	// Matrice 3 !
	Matrix3f m;
	m.Set(0,0,2);
	m.Set(1,0,5);
	m.Set(2,0,7);
	m.Set(0,1,8);
	m.Set(1,1,6);
	m.Set(2,1,1);
	m.Set(0,2,2);
	m.Set(1,2,12);
	m.Set(2,2,14);
	
	float det = m.GetDet();
	if(det != 182)
	{
		cout << "Error matrice3 det !" << endl;
		cout << "det = " << det << endl;
	}
	
	Matrix3f m2 = m.GetInverted();
	if(	(int)( m2.Get(0,0) * 10000 ) !=  3956 ||
			(int)( m2.Get(1,0) * 10000 ) !=   769 ||
			(int)( m2.Get(2,0) * 10000 ) != -2032 ||
			(int)( m2.Get(0,1) * 10000 ) != -6043 ||
			(int)( m2.Get(1,1) * 10000 ) !=   769 ||
			(int)( m2.Get(2,1) * 10000 ) !=  2967 ||
			(int)( m2.Get(0,2) * 10000 ) !=  4615 ||
			(int)( m2.Get(1,2) * 10000 ) != - 769 ||
			(int)( m2.Get(2,2) * 10000 ) != -1538 )
	{
		cout << "Error matrice3 inversion !" << endl;
		cout << "m2 = " << m2.ToString() << endl;
	}
	
	float det4 = matA.GetDet();
	if(det4 != 460432)
	{
		cout << "Error matrice4 det !" << endl;
		cout << "det = " << det4 << endl;
	}
	
	Matrix4f matD = matA.GetInverted();
	if(	(int)( matD.Get(0,0) * 10000) != -119 ||
			(int)( matD.Get(1,0) * 10000) != 485 ||
			(int)( matD.Get(2,0) * 10000) != 13 ||
			(int)( matD.Get(3,0) * 10000) != -127 ||
			(int)( matD.Get(0,1) * 10000) != -461 ||
			(int)( matD.Get(1,1) * 10000) != 695 ||
			(int)( matD.Get(2,1) * 10000) != 8 ||
			(int)( matD.Get(3,1) * 10000) != 46 ||
			(int)( matD.Get(0,2) * 10000) != -29 ||
			(int)( matD.Get(1,2) * 10000) != -94 ||
			(int)( matD.Get(2,2) * 10000) != 134 ||
			(int)( matD.Get(3,2) * 10000) != 19 ||
			(int)( matD.Get(0,3) * 10000) != 2457 ||
			(int)( matD.Get(1,3) * 10000) != -3954 ||
			(int)( matD.Get(2,3) * 10000) != -148 ||
			(int)( matD.Get(3,3) * 10000) != 1006	)
	{
		cout << "Error matrice inversion !" << endl;
		cout << "Inverse = " << matD.ToString() << endl;
	}


	
}
			
void test_vector()
{
	cout << "---- Test Vector ----" << endl;
	
	Vector4f v;
	v.Set(1,0,1,0);
	Vector4f v2 = v.GetNormalized();
	if(v2.GetLength() != 1)
	{
		cout << "Error vector4 normalization !" << endl;
		cout << "|v2| = " << v2.GetLength() << endl;
		cout << "v2 = " << v2.ToString() << endl;
	}
	
	Vector4f v3 = v + v;
	if(v3.GetX() != 2 || v3.GetY() != 0 || v3.GetZ() != 2 || v3.GetW() != 0)
	{
		cout << "Error vector4 addition !" << endl;
		cout << "v3 = " << v3.ToString() << endl;
	}
	v3 = v3 * 2;
	if(v3.GetX() != 4 || v3.GetY() != 0 || v3.GetZ() != 4 || v3.GetW() != 0)
	{
		cout << "Error vector4 multiplication !" << endl;
		cout << "v3 = " << v3.ToString() << endl;
	}
	
	Vector4f v4, v5;
	v4.Set(24, 4, 7, 9);
	v5.Set(1, 6, 4, 3);
	float dot = v4.Dot(v5);
	if(dot != 103)
	{
		cout << "Error vector4 dot product !" << endl;
		cout << "Dot product = " << dot << endl;
	}
	
	Vector4f v6 = v4.Cross(v5);
	if(v6.GetX() != -26 || v6.GetY() != -89 || v6.GetZ() != 140 || v6.GetW() != 0)
	{
		cout << "Error vector4 cross product !" << endl;
		cout << "Cross product = " << v6.ToString() << endl;
	}
	
}
			
void maths_unit_tests()
{
	cout << "Unit Tests Starts" << endl;
	test_vector();
	test_matrix();
}
